#include "lidar_sim_core.h"

LidarSim::LidarSim():nh_(""), private_nh_("~"), tf_listener(buffer){
    private_nh_.param<std::string>("output_pc2_topic_", output_pc2_topic_, "");
    private_nh_.param<double>("radius", radius, 0.6);
    x = 0.0;
    y = 0.0;
    frame_id = "map";
}

LidarSim::~LidarSim(){}

void LidarSim::click_point_callback(const geometry_msgs::PointStamped::ConstPtr& msg){
    x = msg->point.x;
    y = msg->point.y;
    // frame_id = msg->header.frame_id;
}

void LidarSim::run(){
    pointcloud2_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_pc2_topic_, 1);
    click_point_sub_ = nh_.subscribe<geometry_msgs::PointStamped>("clicked_point", 1, &LidarSim::click_point_callback, this);
    ros::Timer timer = nh_.createTimer(ros::Duration(0.1), &LidarSim::process, this, false, true);
    ros::spin();
}

void LidarSim::process(const ros::TimerEvent& e){
     pcl::PointCloud<pcl::PointXYZ> cloud;
    sensor_msgs::PointCloud2 output;;
    cloud.width  = 20;
    cloud.height = 1;
    cloud.points.resize(cloud.width * cloud.height);
    for(int i=1;i<=5;i++){
        double p_x, p_y;
        p_x = x;
        p_y = y+i*radius/5.0;
        cloud.points[i-1].x = p_x;
        cloud.points[i-1].y = p_y;
    }

    for(int i=1;i<=5;i++){
        double p_x, p_y;
        p_x = x-i*radius/5.0;
        p_y = y;
        cloud.points[5+i-1].x = p_x;
        cloud.points[5+i-1].y = p_y;
    }

    for(int i=1;i<=5;i++){
        double p_x, p_y;
        p_x = x;
        p_y = y-i*radius/5.0;
        cloud.points[10+i-1].x = p_x;
        cloud.points[10+i-1].y = p_y;
    }

    for(int i=1;i<=5;i++){
        double p_x, p_y;
        p_x = x+i*radius/5.0;
        p_y = y;
        cloud.points[15+i-1].x = p_x;
        cloud.points[15+i-1].y = p_y;
    }
    pcl::toROSMsg(cloud, output);
    output.header.frame_id = frame_id;
    output.header.stamp = ros::Time().now();

    /* 将点云转换到lidar坐标系上 */
    if(buffer.canTransform("lidar", "map", ros::Time(0))){
        geometry_msgs::Transform concat_transform;
        geometry_msgs::TransformStamped concat_transform_stamed = buffer.lookupTransform("lidar", "map", ros::Time(0));
        concat_transform.translation = concat_transform_stamed.transform.translation;
        concat_transform.rotation = concat_transform_stamed.transform.rotation;
        Eigen::Matrix4f concat_eigen_transform;
        pcl_ros::transformAsMatrix(concat_transform, concat_eigen_transform);
        pcl_ros::transformPointCloud(concat_eigen_transform, output, output);
        output.header.frame_id = "lidar";
        pointcloud2_pub_.publish(output);
    }
}